d1abd5502b215f56ec5ffcc21a8386e8fba4c588,Atlas/src/us/ihmc/atlas/StepAdjustmentVisualizers/StepAdjustmentExampleGraphic.java,StepAdjustmentExampleGraphic,initialize,#,259

Before Change


      icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
      icpOptimizationController.addFootstepToPlan(nextFootstep);
      icpOptimizationController.addFootstepToPlan(nextNextFootstep);
      icpOptimizationController.addFootstepToPlan(nextNextNextFootstep);

      RobotSide supportSide = nextFootstep.getRobotSide().getOppositeSide();

      icpPlanner.setSupportLeg(supportSide);
      icpPlanner.initializeForTransfer(yoTime.getDoubleValue());
      icpPlanner.getDesiredCapturePointPositionAndVelocity(desiredICP, desiredICPVelocity, yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue());

      icpOptimizationController.initializeForTransfer(yoTime.getDoubleValue(), supportSide, omega0.getDoubleValue());
      icpOptimizationController.compute(yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue(), desiredICP, desiredICPVelocity, desiredICP, omega0.getDoubleValue());

      /** do single support **/

      FootSpoof footSpoof = contactableFeet.get(supportSide.getOppositeSide());
      FramePose nextSupportPose = footPosesAtTouchdown.get(supportSide.getOppositeSide());
      nextSupportPose.setToZero(nextFootstep.getSoleReferenceFrame());
      nextSupportPose.changeFrame(ReferenceFrame.getWorldFrame());
      footSpoof.setSoleFrame(nextSupportPose);

      contactStates.get(supportSide.getOppositeSide()).clear();
      if (nextFootstep.getPredictedContactPoints() == null)
         contactStates.get(supportSide.getOppositeSide()).setContactFramePoints(footSpoof.getContactPoints2d());
      else
         contactStates.get(supportSide.getOppositeSide()).setContactPoints(nextFootstep.getPredictedContactPoints());
      bipedSupportPolygons.updateUsingContactStates(contactStates);

      icpPlanner.clearPlan();
      icpOptimizationController.clearPlan();

      timing.setTimings(singleSupportDuration.getDoubleValue(), doubleSupportDuration.getDoubleValue());
      icpPlanner.addFootstepToPlan(nextFootstep, timing);
      icpPlanner.addFootstepToPlan(nextNextFootstep, timing);
      icpPlanner.addFootstepToPlan(nextNextNextFootstep, timing);

      icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
      icpOptimizationController.addFootstepToPlan(nextFootstep);
      icpOptimizationController.addFootstepToPlan(nextNextFootstep);
      icpOptimizationController.addFootstepToPlan(nextNextNextFootstep);

      icpPlanner.setSupportLeg(supportSide);
      icpPlanner.initializeForSingleSupport(yoTime.getDoubleValue());

After Change



      icpOptimizationController.clearPlan();
      icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(0));
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(1));
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(2));

      RobotSide supportSide = plannedFootsteps.get(0).getRobotSide().getOppositeSide();

      icpPlanner.setSupportLeg(supportSide);
      icpPlanner.initializeForTransfer(yoTime.getDoubleValue());
      icpPlanner.getDesiredCapturePointPositionAndVelocity(desiredICP, desiredICPVelocity, yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue());

      icpOptimizationController.initializeForTransfer(yoTime.getDoubleValue(), supportSide, omega0.getDoubleValue());
      icpOptimizationController.compute(yoTime.getDoubleValue() + doubleSupportDuration.getDoubleValue(), desiredICP, desiredICPVelocity, desiredICP, omega0.getDoubleValue());

      /** do single support **/

      FootSpoof footSpoof = contactableFeet.get(supportSide.getOppositeSide());
      FramePose nextSupportPose = footPosesAtTouchdown.get(supportSide.getOppositeSide());
      nextSupportPose.setToZero(plannedFootsteps.get(0).getSoleReferenceFrame());
      nextSupportPose.changeFrame(ReferenceFrame.getWorldFrame());
      footSpoof.setSoleFrame(nextSupportPose);

      contactStates.get(supportSide.getOppositeSide()).clear();
      if (plannedFootsteps.get(0).getPredictedContactPoints() == null)
         contactStates.get(supportSide.getOppositeSide()).setContactFramePoints(footSpoof.getContactPoints2d());
      else
         contactStates.get(supportSide.getOppositeSide()).setContactPoints(plannedFootsteps.get(0).getPredictedContactPoints());
      bipedSupportPolygons.updateUsingContactStates(contactStates);

      icpPlanner.clearPlan();
      timing.setTimings(singleSupportDuration.getDoubleValue(), doubleSupportDuration.getDoubleValue());
      icpPlanner.addFootstepToPlan(plannedFootsteps.get(0), timing);
      icpPlanner.addFootstepToPlan(plannedFootsteps.get(1), timing);
      icpPlanner.addFootstepToPlan(plannedFootsteps.get(2), timing);

      icpOptimizationController.clearPlan();
      icpOptimizationController.setStepDurations(doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(0));
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(1));
      icpOptimizationController.addFootstepToPlan(plannedFootsteps.get(2));